import cv2, threading, time
from typing import Any, Optional, Dict
from src.driver.base import ICamera
from src.data.structures import BinocularImagePair


# ------------------------------------------------------------------
# 无锁双缓冲最新帧指针
# ------------------------------------------------------------------
class _LatestFrame:
    """单写多读无锁指针"""

    __slots__ = ("_buffer", "_write_idx")

    def __init__(self) -> None:
        self._buffer: list[Optional[BinocularImagePair]] = [None, None]
        self._write_idx: int = 0  # 只被写线程修改

    # ---------- 写端（单线程）----------
    def write(self, pair: BinocularImagePair) -> None:
        """后台采集线程：原子写 + 切换槽"""
        self._buffer[self._write_idx] = pair
        self._write_idx ^= 1  # 原子切换

    # ---------- 读端（任意线程）----------
    def read(self) -> Optional[BinocularImagePair]:
        """前台任意线程：无锁读最新帧"""
        # 读「上一次写完」的槽
        return self._buffer[self._write_idx ^ 1]


# ------------------------------------------------------------------
# 无锁 USB 相机
# ------------------------------------------------------------------
class StereoCamera_USB(ICamera):
    def __init__(self, left_id: int, right_id: int):
        self._cap_left = cv2.VideoCapture(left_id)
        self._cap_right = cv2.VideoCapture(right_id)
        # 1. 无锁双缓冲
        self._latest = _LatestFrame()
        self._stop = threading.Event()
        # 2. 统计信息（无锁读）
        self._drops = 0

    def open(
        self,
        width: int | None = None,
        height: int | None = None,
        fps: int | None = None,
    ) -> bool:
        for cap in (self._cap_left, self._cap_right):
            if width is not None and height is not None:
                cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
                cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
            if fps is not None:
                cap.set(cv2.CAP_PROP_FPS, fps)
            if not cap.isOpened():
                return False
        # 读取真实尺寸
        self._width = int(self._cap_left.get(cv2.CAP_PROP_FRAME_WIDTH))
        self._height = int(self._cap_left.get(cv2.CAP_PROP_FRAME_HEIGHT))
        self._thread = threading.Thread(target=self._captureLoop, daemon=True)
        self._thread.start()
        return True

    def close(self):
        self._stop.set()
        self._thread.join()
        self._cap_left.release()
        self._cap_right.release()

    # ---------------- 采集线程(单写)----------------
    def _captureLoop(self):
        while not self._stop.is_set():
            ok1, f1 = self._cap_left.read()
            ok2, f2 = self._cap_right.read()
            if ok1 and ok2:
                self._latest.write(BinocularImagePair(f1, f2, timestamp=time.time()))
            else:
                self._drops += 1

    # ---------------- 前台接口(多读)----------------
    def grab(self) -> BinocularImagePair:
        """无锁读最新帧(阻塞直到有帧)"""
        while True:
            latest = self._latest.read()
            if latest is not None:
                return latest
            time.sleep(0.001)  # 避免 CPU 空转

    @property
    def properties(self) -> Dict[str, Any]:
        # 无锁读统计
        return {"width": self._width, "height": self._height, "drops": self._drops}
